#include "start.h"

.globl _start
_start:	b	start_code
	ldr	pc, _undefined_instruction
	ldr	pc, _software_interrupt
	ldr	pc, _prefetch_abort
	ldr	pc, _data_abort
	ldr	pc, _not_used
	ldr	pc, _irq
	ldr	pc, _fiq

_undefined_instruction:	.word undefined_instruction
_software_interrupt:	.word software_interrupt
_prefetch_abort:	.word prefetch_abort
_data_abort:		.word data_abort
_not_used:		.word not_used
_irq:			.word irq
_fiq:			.word fiq


	.balignl 16,0xdeadbeef

/*
 *************************************************************************
 *
 * Startup Code (called from the ARM reset exception vector)
 *
 * do important init only if we don't start from memory!
 * relocate armboot to ram
 * setup stack
 * jump to second stage
 *
 *************************************************************************
 */
.globl _TEXT_BASE
_TEXT_BASE:
	.word	TEXT_BASE

.globl _armboot_start
_armboot_start:
	.word _start
		



/*
 * These are defined in the board-specific linker script.
 */
.globl _bss_start
_bss_start:
	.word __bss_start

.globl _bss_end
_bss_end:
	.word __bss_end



/*
 * the actual start code
 */
start_code:
	/*
	 * set the cpu to SVC32 mode
	 */
	 
	/* Interupt-Controller base addresses */

	mrs	r0, cpsr
	bic	r0, r0, #0x1f
	orr	r0, r0, #0xd3
	msr	cpsr, r0

	/* turn off the watchdog */
#define rWTCON		0x53000000
	ldr	r0, =rWTCON
	mov	r1, #0x0
	str	r1, [r0]

	/*
	 * mask all IRQs by setting all bits in the INTMR - default
	 */
#define rINTMSK		0x4A000008
	mov	r1, #0xffffffff
	ldr	r0, =rINTMSK
	str	r1, [r0]


#define rINTSUBMSK	0x4A00001C
    ldr     r1, =0x7fff
    ldr     r0, =rINTSUBMSK
    str     r1, [r0]

    /* Set system clock */
	/* FCLK:HCLK:PCLK = 1:4:8 */
	/* default FCLK is 400 MHz ! */
	/* clock divisor register */

#define rCLKDIVN	0x4C000014	
#define	rMPLLCON	0x4c000004
#define rUPLLCON 	0x4c000008

	ldr	r0, =rCLKDIVN
	mov	r1, #5
	str	r1, [r0]

	ldr r0, =rMPLLCON
	ldr r1, =(92<<12)|(1<<4)|(1)
	str r1, [r0]
	
	/*   asynchronous    mode*/
    @ means Fclk:Hclk is not 1:1
	mrc p15,0,r0,c1,c0,0
	orr r0,r0,#0xc0000000;
	mcr p15,0,r0,c1,c0,0

	/* set UPLLCON */
	ldr r0, =rUPLLCON
	ldr r1, =(0x38<<12)|(0x02<<4)|(0x02)
	str r1, [r0]

#define rGPBCON		0x56000010
#define rGPBUP		0x56000018
#define rGPBDAT		0x56000014

	/*
	 * we do sys-critical inits only at reboot,
	 * not when booting from ram!
	 */
#ifndef CONFIG_SKIP_LOWLEVEL_INIT
	bl	cpu_init_crit
#endif

	.macro Chang_Mode_To_SysMode			
	mrs r4, cpsr				 
	bic r4, r4, #0x1f
	orr r4, r4, #0xdf
	msr cpsr, r4
	.endm
	.macro Chang_Mode_To_SvcMode			
	mrs r4, cpsr				 
	bic r4, r4, #0x1f
	orr r4, r4, #0xd3
	msr cpsr, r4
	.endm
/*
 *************************************************************************
 *
 * Check boot flash
 *
 *************************************************************************
 */
 #define rBWSCON 	0x48000000
	Chang_Mode_To_SysMode
	ldr 	sp, SYSTEM_STACK_START		@ setup stack pointer
	Chang_Mode_To_SvcMode
	mov     r0, #rBWSCON 
	ldr     r0, [r0]
	bic     r0, r0, #0xfffffff9  /* BWSCON[2:1] is controled by OM[1:0] */
	cmp     r0, #0               /* when OM[1:0] is 00,BSWCON[2:1]=00, nand flash boot */
	bne     norrelocate             /* norflash boot */

/*
 *************************************************************************
 *
 *  relocate U-Boot From Nandflash to RAM	    
 *
 *************************************************************************
 */
 	Chang_Mode_To_SysMode
	bl NF_Init
	Chang_Mode_To_SvcMode	
	adr	r0, _start		/* r0 <- current position of code   */
	ldr	r1, _TEXT_BASE		/* the dest position of code */
	/* LENGTH_UBOOT is the size of source */
	ldr	r2, _armboot_start
	ldr	r3, _bss_start
	sub	r2, r3, r2		/* r2 <- size of armboot            */
	Chang_Mode_To_SysMode
	bl RdNF2SDRAM_NoEcc
	Chang_Mode_To_SvcMode
	bl ClearBssSection
/*
 *************************************************************************
 *
 *	relocate U-Boot From Norflash to RAM		
 *
 *************************************************************************
 */

norrelocate:
	
	adr	r0, _start		/* r0 <- current position of code   */
	ldr	r1, _TEXT_BASE		/* test if we run from flash or RAM */
	cmp	r0, r1			/* don't reloc during debug         */
	beq	Jump_main
	ldr	r2, _armboot_start
	ldr	r3, _bss_start
	sub	r2, r3, r2		/* r2 <- size of armboot            */
	add	r2, r0, r2		/* r2 <- source end address         */
	Chang_Mode_To_SysMode
	bl CopyNorCode2Sdram
	Chang_Mode_To_SvcMode
/*
 *************************************************************************
 *
 * Clear Bss Section
 *
 *************************************************************************
 */
ClearBssSection:

	bl clear_bss

	


/*
 *************************************************************************
 *
 * Jump to main function
 *
 *************************************************************************
 */
Jump_main:
	Chang_Mode_To_SysMode
	ldr	pc, _main
_main:	.word main
/*
 ************************************************************************
 * CPU_init_critical registers
 *
 * setup important registers
 * setup memory timing
 *
 *************************************************************************
 */
#ifndef CONFIG_SKIP_LOWLEVEL_INIT
cpu_init_crit:
	/*
	 * flush v4 I/D caches
	 */
	mov	r0, #0
	mcr	p15, 0, r0, c7, c7, 0	/* flush v3/v4 cache */
	mcr	p15, 0, r0, c8, c7, 0	/* flush v4 TLB */

	/*
	 * disable MMU stuff and caches
	 */
	mrc	p15, 0, r0, c1, c0, 0
	bic	r0, r0, #0x00002300	@ clear bits 13, 9:8 (--V- --RS)
	bic	r0, r0, #0x00000087	@ clear bits 7, 2:0 (B--- -CAM)
	orr	r0, r0, #0x00000002	@ set bit 2 (A) Align
	orr	r0, r0, #0x00001000	@ set bit 12 (I) I-Cache
	mcr	p15, 0, r0, c1, c0, 0

	/*
	 * before relocating, we have to setup RAM timing
	 * because memory timing is board-dependend, you will
	 * find a lowlevel_init.S in your board directory.
	 */
	mov	ip, lr

	bl	lowlevel_init

	mov	lr, ip
	mov	pc, lr
#endif /* CONFIG_SKIP_LOWLEVEL_INIT */


/*
 *************************************************************************
 *
 * Interrupt handling
 *
 *************************************************************************
 */

@
@ IRQ stack frame.
@
#define S_FRAME_SIZE	72
	
#define S_OLD_R0	68
#define S_PSR		64
#define S_PC		60
#define S_LR		56
#define S_SP		52
	
#define S_IP		48
#define S_FP		44
#define S_R10		40
#define S_R9		36
#define S_R8		32
#define S_R7		28
#define S_R6		24
#define S_R5		20
#define S_R4		16
#define S_R3		12
#define S_R2		8
#define S_R1		4
#define S_R0		0
	
#define MODE_SVC	0x13
#define I_BIT		0x80
/*
 *************************************************************************
 *
 * Set up Stack
 *
 *************************************************************************
 */
/*
 *	Stack position
 */
SYSTEM_STACK_START: .word (system_stack_start + SYSTEM_STACK)
#if ((system_stack_start + SYSTEM_STACK) & 7)
#error SYSTEM_STACK_START not align to 8 bytes
#endif
IRQ_STACK_START: .word (irq_stack_start + IRQ_STACK)
#if ((irq_stack_start + IRQ_STACK) & 7)
#error IRQ_STACK_START not align to 8 bytes
#endif
SVC_STACK_START: .word (svc_stack_start + SVC_STACK)
#if ((svc_stack_start + SVC_STACK) & 7)
#error SVC_STACK_START not align to 8 bytes
#endif
FIQ_STACK_START: .word (fiq_stack_start + FIQ_STACK)
#if ((fiq_stack_start + FIQ_STACK) & 7)
#error FIQ_STACK_START not align to 8 bytes
#endif
BAD_STACK_START: .word (bad_stack_start + BAD_STACK)
#if ((bad_stack_start + BAD_STACK) & 7)
#error BAD_STACK_START not align to 8 bytes
#endif


	.macro get_irq_stack			@ setup IRQ stack
	ldr	sp, IRQ_STACK_START
	.endm
	
	.macro get_svc_stack			@ setup IRQ stack
	ldr	sp, SVC_STACK_START
	.endm
	

	.macro get_fiq_stack			@ setup IRQ stack
	ldr	sp, FIQ_STACK_START
	.endm

/*
 * use bad_save_user_regs for abort/prefetch/undef/swi ...
 * use irq_save_user_regs / irq_restore_user_regs for IRQ/FIQ handling
 */
	.macro	bad_save_user_regs
	sub	sp, sp, #S_FRAME_SIZE
	stmia	 sp, {r0 - r12}			@ Calling r0-r12
	ldr	r2, BAD_STACK_START
	ldmia	r2, {r2 - r3}			@ get pc, cpsr
	add	r0, sp, #S_FRAME_SIZE		@ restore sp_SVC

	add	r5, sp, #S_SP
	mov	r1, lr
	stmia	 r5, {r0 - r3}			@ save sp_SVC, lr_SVC, pc, cpsr
	mov	r0, sp
	.endm


	.macro get_bad_stack
	ldr	r13, BAD_STACK_START		@ setup our mode stack

	str	lr, [r13]			@ save caller lr / spsr
	mrs	lr, spsr
	str	lr, [r13, #4]

	mov	r13, #MODE_SVC			@ prepare SVC-Mode
	@ msr	spsr_c, r13
	msr	spsr, r13
	mov	lr, pc
	movs	pc, lr  @restore PC and CPSR 
	.endm	

	.macro	irq_save_user_regs
	sub sp, sp, #S_FRAME_SIZE
	stmia sp, {r0 - r12}			@ Calling r0-r12
	add r7, sp, #S_PC
	stmdb	r7, {sp, lr}^			@ Calling SP, LR
	str lr, [r7, #0]			@ Save calling PC
	mrs r6, spsr
	str r6, [r7, #4]			@ Save CPSR
	str r0, [r7, #8]			@ Save OLD_R0
	mov r0, sp
	.endm

	.macro	irq_restore_user_regs
	ldmia	sp, {r0 - lr}^			@ Calling r0 - lr
	mov r0, r0
	ldr lr, [sp, #S_PC] 		@ Get PC
	add sp, sp, #S_FRAME_SIZE
	/* return & move spsr_svc into cpsr */
	subs	pc, lr, #4
	.endm


/*
 * exception handlers
 */
	.align	5
undefined_instruction:
	//bl undefined_instruction
	get_bad_stack
	bad_save_user_regs
	bl	do_undefined_instruction

	.align	5
software_interrupt:

	get_svc_stack
	stmfd  sp!, {r0-r12, lr}

	ldr    r0, [lr, #-4]
	bic    r0, r0, #0xff000000
	bl	  Swi_Bulk_Transfer_Handler

	ldmfd	sp!,  {r0-r12, pc}^


	//get_bad_stack
	//bad_save_user_regs
	//bl	do_software_interrupt

	.align	5
prefetch_abort:
	//bl prefetch_abort
	get_bad_stack
	bad_save_user_regs
	bl	do_prefetch_abort

	.align	5
data_abort:
	//bl data_abort
	get_bad_stack
	bad_save_user_regs
	bl	do_data_abort

	.align	5
not_used:
	//bl not_used
	get_bad_stack
	bad_save_user_regs
	bl	do_not_used

	
	.align	5
irq:
	get_irq_stack
	/* someone ought to write a more effiction fiq_save_user_regs */
	irq_save_user_regs
	#define rINTOFFSET 0x4a000014	

	LDR     R0, =rINTOFFSET
    LDR     R0, [R0]
   
    //LDR     R1, =HandleEINT0
    MOV     LR, PC                          @ Save LR befor jump to the C function we need return back
    //LDR     PC, [R1, R0, LSL #2]            @ Call OS_CPU_IRQ_ISR_handler(); 
    b	asm_do_IRQ
	irq_restore_user_regs
		
	.align	5
fiq:
	bl fiq
	//get_fiq_stack
	/* someone ought to write a more effiction fiq_save_user_regs */
	//irq_save_user_regs
	//bl 	do_fiq
	//irq_restore_user_regs	


/*
 *************************************************************************
 *
 * Exception Vection
 *
 *************************************************************************
 */

.equ	ISR_BADDR,	0x33ffff00        

HandleReset:
 	.word		(ISR_BADDR+4*0)
HandleUndef:
	.word		(ISR_BADDR+4*1)
HandleSWI:
	.word		(ISR_BADDR+4*2)
HandlePabort:
	.word		(ISR_BADDR+4*3)
HandleDabort:
	.word		(ISR_BADDR+4*4)
HandleReserved:
	.word		(ISR_BADDR+4*5)
HandleIRQ:
	.word		(ISR_BADDR+4*6)	
HandleFIQ:
	.word		(ISR_BADDR+4*7)

.globl  HandleEINT0 
.equ	HandleEINT0,		(ISR_BADDR+4*8)
.equ	HandleEINT1,		(ISR_BADDR+4*9)
.equ	HandleEINT2,		(ISR_BADDR+4*10)
.equ	HandleEINT3,		(ISR_BADDR+4*11)
.equ	HandleEINT4_7,		(ISR_BADDR+4*12)
.equ	HandleEINT8_23,		(ISR_BADDR+4*13)
.equ	HandleRSV6,		(ISR_BADDR+4*14)
.equ	HandleBATFLT,		(ISR_BADDR+4*15)
.equ	HandleTICK,		(ISR_BADDR+4*16)
.equ	HandleWDT,		(ISR_BADDR+4*17)
.equ	HandleTIMER0,		(ISR_BADDR+4*18)
.equ	HandleTIMER1,		(ISR_BADDR+4*19)
.equ	HandleTIMER2,		(ISR_BADDR+4*20)
.equ	HandleTIMER3,		(ISR_BADDR+4*21)
.equ	HandleTIMER4,		(ISR_BADDR+4*22)
.equ	HandleUART2,		(ISR_BADDR+4*23)
.equ	HandleLCD,		(ISR_BADDR+4*24)
.equ	HandleDMA0,		(ISR_BADDR+4*25)
.equ	HandleDMA1,		(ISR_BADDR+4*26)
.equ	HandleDMA2,		(ISR_BADDR+4*27)
.equ	HandleDMA3,		(ISR_BADDR+4*28)
.equ	HandleMMC,		(ISR_BADDR+4*29)
.equ	HandleSPI0,		(ISR_BADDR+4*30)
.equ	HandleUART1,		(ISR_BADDR+4*31)
.equ	HandleRSV24,		(ISR_BADDR+4*32)
.equ	HandleUSBD,		(ISR_BADDR+4*33)
.equ	HandleUSBH,		(ISR_BADDR+4*34)
.equ	HandleIIC,		(ISR_BADDR+4*35)
.equ	HandleUART0 ,		(ISR_BADDR+4*36)
.equ	HandleSPI1,		(ISR_BADDR+4*37)
.equ	HandleRTC,		(ISR_BADDR+4*38)
.equ	HandleADC,		(ISR_BADDR+4*39)
